/*
Copyright 2007 Luke Benstead

This file is part of slMath.

slMath is free software; you can redistribute it and/or modify
it under the terms of the GNU Lesser Public License as published by
the Free Software Foundation; either version 3 of the License, or
(at your option) any later version.

slMath is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
GNU Lesser Public License for more details.

You should have received a copy of the GNU Lesser Public License
along with this program.  If not, see <http://www.gnu.org/licenses/>.

*/
#ifdef __CPLUSPLUS
	#include <cassert>
	#include <cstdlib>
#else
	#include <assert.h>
	#include <memory.h>
#endif

#include "math/utility.h"
#include "math/vec4.h"
#include "math/mat4.h"
#include "math/vec3.h"

#ifdef __CPLUSPLUS
	slVec3::slVec3(slScalar x, slScalar y, slScalar z):
	m_X(x),
	m_Y(y),
	m_Z(z) {
		
	}
	
	slVec3::slVec3():
	m_X(0),
	m_Y(0),
	m_Z(0) {
		
	}
#endif

///< Returns the length of the vector
slScalar slVec3Length(const slVec3* pIn)
{
	return slSqrt(slSQR(pIn->m_X) + slSQR(pIn->m_Y) + slSQR(pIn->m_Z));
}

///< Returns the square of the length of the vector
slScalar slVec3LengthSq(const slVec3* pIn)
{
	return slSQR(pIn->m_X) + slSQR(pIn->m_Y) + slSQR(pIn->m_Z);
}

 ///< Returns the vector passed in set to unit length
slVec3* slVec3Normalize(slVec3* pOut, const slVec3* pIn)
{
	slScalar l = 1.0 / slVec3Length(pIn);

	slVec3 v;
	v.m_X = pIn->m_X * l;
	v.m_Y = pIn->m_Y * l;
	v.m_Z = pIn->m_Z * l;

	pOut->m_X = v.m_X;
	pOut->m_Y = v.m_Y;
	pOut->m_Z = v.m_Z;

	return pOut;
}

///< Returns a vector perpendicular to 2 other vectors
slVec3* slVec3Cross(slVec3* pOut, const slVec3* pV1, const slVec3* pV2)
{

	slVec3 v;

	v.m_X = (pV1->m_Y * pV2->m_Z) - (pV1->m_Z * pV2->m_Y);
	v.m_Y = (pV1->m_Z * pV2->m_X) - (pV1->m_X * pV2->m_Z);
	v.m_Z = (pV1->m_X * pV2->m_Y) - (pV1->m_Y * pV2->m_X);

	pOut->m_X = v.m_X;
	pOut->m_Y = v.m_Y;
	pOut->m_Z = v.m_Z;

	return pOut;
}

///< Returns the cosine of the angle between 2 vectors
slScalar slVec3Dot(const slVec3* pV1, const slVec3* pV2)
{
	return (  pV1->m_X * pV2->m_X
			+ pV1->m_Y * pV2->m_Y
			+ pV1->m_Z * pV2->m_Z );
}

///< Adds 2 vectors and returns the result
slVec3* slVec3Add(slVec3* pOut, const slVec3* pV1, const slVec3* pV2)
{
	slVec3 v;

	v.m_X = pV1->m_X + pV2->m_X;
	v.m_Y = pV1->m_Y + pV2->m_Y;
	v.m_Z = pV1->m_Z + pV2->m_Z;

	pOut->m_X = v.m_X;
	pOut->m_Y = v.m_Y;
	pOut->m_Z = v.m_Z;

	return pOut;
}

 ///< Subtracts 2 vectors and returns the result
slVec3* slVec3Subtract(slVec3* pOut, const slVec3* pV1, const slVec3* pV2)
{
	slVec3 v;

	v.m_X = pV1->m_X - pV2->m_X;
	v.m_Y = pV1->m_Y - pV2->m_Y;
	v.m_Z = pV1->m_Z - pV2->m_Z;

	pOut->m_X = v.m_X;
	pOut->m_Y = v.m_Y;
	pOut->m_Z = v.m_Z;

	return pOut;
}

 ///< Transforms vector (x, y, z, 1) by a given matrix.
slVec3* slVec3Transform(slVec3* pOut, const slVec3* pV, const slMat4* pM)
{
	/*
		a = (Vx, Vy, Vz, 1)
		b = (a×M)T
		Out = (bx, by, bz)
	*/

	slVec3 v;

	v.m_X = pV->m_X * pM->m_Mat[0] + pV->m_Y * pM->m_Mat[4] + pV->m_Z * pM->m_Mat[8] + pM->m_Mat[12];
	v.m_Y = pV->m_X * pM->m_Mat[1] + pV->m_Y * pM->m_Mat[5] + pV->m_Z * pM->m_Mat[9] + pM->m_Mat[13];
	v.m_Z = pV->m_X * pM->m_Mat[2] + pV->m_Y * pM->m_Mat[6] + pV->m_Z * pM->m_Mat[10] + pM->m_Mat[14];

	pOut->m_X = v.m_X;
	pOut->m_Y = v.m_Y;
	pOut->m_Z = v.m_Z;

	return pOut;
}

slVec3* slVec3InverseTransform(slVec3* pOut, const slVec3* pVect, const slMat4* pM)
{
	slVec3 v1, v2;

	v1.m_X = pVect->m_X - pM->m_Mat[12];
	v1.m_Y = pVect->m_Y - pM->m_Mat[13];
	v1.m_Z = pVect->m_Z - pM->m_Mat[14];

	v2.m_X = v1.m_X * pM->m_Mat[0] + v1.m_Y * pM->m_Mat[1] + v1.m_Z * pM->m_Mat[2];
	v2.m_Y = v1.m_X * pM->m_Mat[4] + v1.m_Y * pM->m_Mat[5] + v1.m_Z * pM->m_Mat[6];
	v2.m_Z = v1.m_X * pM->m_Mat[8] + v1.m_Y * pM->m_Mat[9] + v1.m_Z * pM->m_Mat[10];

	pOut->m_X = v2.m_X;
	pOut->m_Y = v2.m_Y;
	pOut->m_Z = v2.m_Z;

	return pOut;
}

slVec3* slVec3InverseTransformNormal(slVec3* pOut, const slVec3* pVect, const slMat4* pM)
{
	slVec3 v;

	v.m_X = pVect->m_X * pM->m_Mat[0] + pVect->m_Y * pM->m_Mat[1] + pVect->m_Z * pM->m_Mat[2];
	v.m_Y = pVect->m_X * pM->m_Mat[4] + pVect->m_Y * pM->m_Mat[5] + pVect->m_Z * pM->m_Mat[6];
	v.m_Z = pVect->m_X * pM->m_Mat[8] + pVect->m_Y * pM->m_Mat[9] + pVect->m_Z * pM->m_Mat[10];

	pOut->m_X = v.m_X;
	pOut->m_Y = v.m_Y;
	pOut->m_Z = v.m_Z;

	return pOut;
}

slVec3* slVec3TransformCoord(slVec3* pOut, const slVec3* pV, const slMat4* pM)
{
	/*
        a = (Vx, Vy, Vz, 1)
        b = (a×M)T
        Out = 1⁄bw(bx, by, bz)
	*/

    assert(0);

	return pOut;
}

slVec3* slVec3TransformNormal(slVec3* pOut, const slVec3* pV, const slMat4* pM)
{
/*
    a = (Vx, Vy, Vz, 0)
    b = (a×M)T
    Out = (bx, by, bz)
*/

	slVec3 v;

	v.m_X = pV->m_X * pM->m_Mat[0] + pV->m_Y * pM->m_Mat[4] + pV->m_Z * pM->m_Mat[8];
	v.m_Y = pV->m_X * pM->m_Mat[1] + pV->m_Y * pM->m_Mat[5] + pV->m_Z * pM->m_Mat[9];
	v.m_Z = pV->m_X * pM->m_Mat[2] + pV->m_Y * pM->m_Mat[6] + pV->m_Z * pM->m_Mat[10];

	pOut->m_X = v.m_X;
	pOut->m_Y = v.m_Y;
	pOut->m_Z = v.m_Z;

    return pOut;

}

///< Scales a vector to length s
slVec3* slVec3Scale(slVec3* pOut, const slVec3* pIn, const slScalar s)
{
	slVec3Normalize(pOut, pIn);

	pOut->m_X *= s;
	pOut->m_Y *= s;
	pOut->m_Z *= s;

	return pOut;
}


bool 	slVec3AreEqual(const slVec3* p1, const slVec3* p2)
{
	return (
				(p1->m_X < p2->m_X + slEpsilon && p1->m_X > p2->m_X - slEpsilon) &&
				(p1->m_Y < p2->m_Y + slEpsilon && p1->m_Y > p2->m_Y - slEpsilon) &&
				(p1->m_Z < p2->m_Z + slEpsilon && p1->m_Z > p2->m_Z - slEpsilon)
			);
}
